/*
 *  Extended Kalman Filter estimator
 *  Copyright (C) 2012, CYPHY lab
 *  Inkyu Sa <i.sa@qut.edu.au>
 *
 *  http://wiki.qut.edu.au/display/cyphy
 *
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */



#include "cyphy_ekf_pole_cam_only.h"


#include "jac.c"
#include "line_model.c"


//{

//}
using namespace Eigen;

cyphy_ekf_pole::cyphy_ekf_pole(ros::NodeHandle nh): nh_(nh)
{
	ROS_INFO ("cyphy_ekf_pole");
	dt=0.008;
	fx=267;
	fy=267;
	px=156.8682;
	py=102.6506;
	pole_outSubscriber = nh.subscribe ("pole_dec_out", 1, &cyphy_ekf_pole::poleOutCallback,this);
	mkImuSubscriber = nh.subscribe("mk_imu", 1, &cyphy_ekf_pole::mkImuCallback,this);
	poleInitSubscriber = nh.subscribe ("pole_pose_init", 1, &cyphy_ekf_pole::poleInitCallback,this);
	pub_ekf_odom = nh.advertise<nav_msgs::Odometry>("ekf_mk_odom",1);
	initialized=0;
	
    //nav_sub = nh_.subscribe<ardrone_pose_estimation::Navdata> ("ardrone/navdata", 1, &pose_esti::navCallback, this);
}


cyphy_ekf_pole::~cyphy_ekf_pole()
{

    ROS_INFO ("Destroying cyphy_ekf_pole");
}

void cyphy_ekf_pole::cam_imu_EKF(const Matrix<float,1,4>* measured_line,
	                       const Matrix<float,1,2>* measured_ang,
	                       output* out)
{
   Matrix<float,9,9> out_temp;
   Matrix<float,9,1> xp;
   Matrix<float,9,9> Pp;
   Matrix<float,4,9> H_cam;
   Matrix<float,4,9> H;
   Matrix<float,4,4> s;
   Matrix<float,9,4> K;
   Matrix<float,4,1> v_inn_cam;
   Matrix<float,2,1> v_inn_I;
   Matrix<float,4,1> v_inn;
   
   
   //ROS_INFO ("cam_imu_EKF");
   //cout<<*measured_line<<endl;
   //cout<<*measured_ang<<endl;

   //=========================
   // Prediction
   //=========================
   
   xp = F*x_hat;
   cout<<"xp = "<<xp<<endl;
   Pp = F*P_hat*F.transpose()+Q;

   //=========================
   // Update
   //=========================
   H_cam.setZero();
   H.setZero();
   
   H_cam=Jac_H(xp);

/*   
   for(int i=0;i<4;i++) for(int j=0;j<9;j++) H(i,j)=H_cam(i,j);
   for(int i=0;i<2;i++) for(int j=0;j<9;j++) H(i+4,j)=H_I(i,j);
*/
   for (int i = 0; i < 4; i++) for (int j = 0; j < 9; j++) if (!finite(H_cam(i,j))) H_cam(i,j)=0;
   

   //Get rid of NaN or INF from H
   //for (int i = 0; i < 6; i++) for (int j = 0; j < 9; j++) if (!finite(H(i,j))) H(i,j)=0;
   cout<<"H_cam"<<H_cam<<endl;
   H=H_cam;
   
   s = H*Pp*H.transpose()+R;

   cout<<"s"<<s<<endl;
   
   /*
   MatrixXf S_(6,6);
   S_ << s;
   //MatrixXd A_;
   //EigenSolver<Matrix<float,6,6>> es(s);
   EigenSolver<MatrixXf> es(S_);
//   MatrixXf V(6,6);
//   MatrixXcf V;
//   MatrixXf V;
//   MatrixXf D(6,1);
   MatrixXf inv_D(6,6);   
   MatrixXf inv_s(6,6);

   inv_D.setIdentity(6,6);
   MatrixXf D=es.pseudoEigenvalueMatrix();
   MatrixXf V=es.pseudoEigenvectors();
   for(int i=0;i<6;i++)	inv_D(i,i) = 1/D(i,i);

   inv_s = V*inv_D*V.transpose();
   cout<<"V="<<V<<endl;
   cout<<"D="<<D<<endl;
   cout<<"inv_D ="<<inv_D<<endl;
   cout<<"inv_s="<<inv_s<<endl;
   */

   
   


   
   if(s.determinant()!=0)
   {
	 K=Pp*H.transpose()*s.inverse();
	 cout<<"Pp = "<<Pp<<endl;
	 cout<<"H.transpose() = "<<H.transpose()<<endl;
	 cout<<"s.inverse() = "<<s.inverse()<<endl;



	 cout<<"K = "<<K<<endl;

     	 v_inn_cam=measured_line->transpose()-LineModel_h(xp);
//	 v_inn_I = measured_ang->transpose()-H_I*xp;

//	 for(int i=0;i<4;i++) for(int j=0;j<1;j++) v_inn(i,j)=v_inn_cam(i,j);
//	 for(int i=0;i<2;i++) for(int j=0;j<1;j++) v_inn(i+4,j)=v_inn_I(i,j);
  	 v_inn = v_inn_cam;

	 cout<<"v_inn"<<v_inn<<endl;

	 x_hat = xp+K*v_inn;
	 P_hat = Pp-K*H*Pp;
	 out->y = x_hat;
	 out->covs = P_hat;

	 cout<<"x_hat = "<<x_hat<<endl;
	 //cout<<"P_hat = "<<P_hat<<endl;
                                                                                                                                                                                                                                                                                                                                                                                 	 
	}
	else
	{
	 cout<<"Can't invert"<<endl;
	}
}

void cyphy_ekf_pole::ekf_init()
{
	 Matrix<float,9,1> process_noise;
	 
	 process_noise << pow(0.2,2),pow(0.2,2),pow(0.2,2),
		 	          pow(0.3,2),pow(0.3,2),pow(0.3,2),
		 	          pow(deg2rad(5),2),pow(deg2rad(5),2),pow(deg2rad(1.1),2);
	 
	 //Q = Matrix::Identity(9,9);
	 Q.setIdentity();
	 //cout<<Q<<endl;
	 
	 for(int i=0;i<9;i++) Q(i,i) = process_noise(i);

/*
	 Matrix<float, 6,1> measure_noise;
	 measure_noise<< pow(deg2rad(9),2),pow(4,2),
				 	 pow(deg2rad(9),2),pow(4,2),
				 	 pow(deg2rad(2.5),2),pow(deg2rad(2.5),2);

	 R.setIdentity();
	 for(int i=0;i<6;i++) R(i,i) = measure_noise(i);
*/

	 Matrix<float, 4,1> measure_noise;
	 measure_noise<< pow(deg2rad(6),2),pow(4,2),pow(deg2rad(6),2),pow(4,2);
	 R.setIdentity();
	 for(int i=0;i<4;i++) R(i,i) = measure_noise(i);

	 //cout<<Q<<endl<<R<<endl;
    	 /*
	 x_hat<< -1.0457,-0.12,-0.2330,
	 	        0   ,   0   ,   0   ,
	 	      -0.05236, -0.0038, -0.0073;
	*/
	x_hat<<pole_init_msg.data[0],pole_init_msg.data[1],-0.2330,
		0   ,   0   ,   0   ,
	 	-0.05236, -0.0038, -0.0073;
	cout<<"x_hat = "<<x_hat<<endl;


	 P_hat.setIdentity();
	 
	 Matrix<float, 9,1> init_p;
	 init_p << pow(100,2),pow(100,2),pow(100,2),
	 	       pow(100,2),pow(100,2),pow(100,2),
	 	       pow(deg2rad(50),2),pow(deg2rad(50),2),pow(deg2rad(50),2);
	 for(int i=0;i<9;i++) P_hat(i,i) = init_p(i);

	 F << 1,0,0,dt,0,0,0,0,0,
	 	  0,1,0,0,dt,0,0,0,0,
	 	  0,0,1,0,0,dt,0,0,0,
	 	  0,0,0,1,0,0 ,0,0,0,
	 	  0,0,0,0,1,0 ,0,0,0,
	 	  0,0,0,0,0,1 ,0,0,0,
	 	  0,0,0,0,0,0 ,1,0,0,
	 	  0,0,0,0,0,0 ,0,1,0,
	 	  0,0,0,0,0,0 ,0,0,1;
	 //cout<<F<<endl;
	 //cout<<x_hat<<endl<<P<<endl;
	 H_I << 0,0,0,0,0,0,1,0,0,
	        0,0,0,0,0,0,0,1,0;
}

double cyphy_ekf_pole::deg2rad(double deg) 
{
	return deg * (M_PI / 180);
}

double cyphy_ekf_pole::rad2deg(double rad) 
{
	return rad * (180 / M_PI);	
}

void cyphy_ekf_pole::poleInitCallback(const std_msgs::Float32MultiArray& msg)
{
	//ROS_INFO("poleInitCallBack");
	//cout<<msg.data[0]<<endl<<msg.data[1]<<endl;
	pole_init_msg = msg;
	
}

void cyphy_ekf_pole::poleOutCallback(const std_msgs::Float32MultiArray& msg)
{
	//ROS_INFO("poleOutCallBack");
	if(initialized)
	{
		//for(int i=0;i<4;i++) measured_line(0,i) = msg.data[i];
				
		measured_line(0,0) = msg.data[0];
		measured_line(0,1) = msg.data[1];
		measured_line(0,2) = msg.data[2];
		measured_line(0,3) = msg.data[3];
		
		for(int i=0;i<4;i++) cout<<"measure["<<i<<"] = "<<measured_line(0,i)<<endl;

		measured_ang(0,0) = Imu_msg.orientation.x; // Roll angle in Rad
		measured_ang(0,1) = Imu_msg.orientation.y; // Pitch angle in Rad
		cam_imu_EKF(&measured_line,&measured_ang,&ekf_out);
		//cout << "Time: " << T*1e6 << " us" << endl;
		//cout<<"y = "<<ekf_out.y<<endl;
		//cout<<"cov = "<<ekf_out.covs<<endl;
		ekf_mk_odom_msg.pose.pose.position.x=ekf_out.y(0,0); 	// x in metre
		ekf_mk_odom_msg.pose.pose.position.y=ekf_out.y(1,0);	// y in metre
		ekf_mk_odom_msg.pose.pose.position.z=ekf_out.y(2,0);	// z in metre
		ekf_mk_odom_msg.twist.twist.linear.x=ekf_out.y(3,0);	// vel x in metre/sec
		ekf_mk_odom_msg.twist.twist.linear.y=ekf_out.y(4,0);	// vel y in metre/sec
		ekf_mk_odom_msg.twist.twist.linear.z=ekf_out.y(5,0);	// vel z in metre/sec

		//Publish angles as Quaternion form.	
		/*
	 	btQuaternion q;
		q.setEulerZYX(y(8),y(7),y(6));
		tf::quaternionTFToMsg(q,ekf_mk_odom_msg.pose.pose.orientation);
		*/

		ekf_mk_odom_msg.pose.pose.orientation.x=ekf_out.y(6,0);  	//roll in Rad
		ekf_mk_odom_msg.pose.pose.orientation.y=ekf_out.y(7,0);  	//pitch in Rad
		ekf_mk_odom_msg.pose.pose.orientation.z=ekf_out.y(8,0);  	//yaw in Rad
		ekf_mk_odom_msg.pose.pose.orientation.w=0.2;	//empty

		
		ekf_mk_odom_msg.header.stamp=ros::Time::now();
		pub_ekf_odom.publish(ekf_mk_odom_msg);
	}
	else
	{ 
		ekf_init();
		initialized=1;
	}
		
}

void cyphy_ekf_pole::mkImuCallback(const sensor_msgs::Imu& msg)
{
	//ROS_INFO("mkImuCallback");	
	Imu_msg = msg;
	//measured_ang(0,0) = Imu_msg.orientation.x; //roll in rad
	//measured_ang(0,1) = Imu_msg.orientation.y; //roll in rad
	measured_ang(0,0) = 0;//Imu_msg.orientation.x; //roll in rad
	measured_ang(0,1) = 0;//Imu_msg.orientation.y; //roll in rad
}



Matrix<float,4,9> cyphy_ekf_pole::Jac_H(Matrix<float,9,1>x)
{
  Matrix<float,4,9> ret;
  ret.setZero();
  double jac_out0[2][9];
  double jac_out1[2][9];
  double (*jac_pt0)[9];
  double (*jac_pt1)[9];
  jac_pt0 = jac_out0;
  jac_pt1 = jac_out1;
  
  double Ax,Ay,Az,Bx,By,Bz,Cx,Cy,Cz,Dx,Dy,Dz,c_x,c_y,c_z,c_dx,c_dy,c_dz,phi,theta,psi;

  
  Ax=0;Ay=0;Az=0;
  Bx=0;By=0;Bz=-0.3;
  Cx=0;Cy=0.16;Cz=0;
  Dx=0;Dy=0.16;Dz=-0.3;
  
  c_x=x(0);c_y=x(1);c_z=x(2);
  c_dx=x(3);c_dy=x(4);c_dz=x(5);
  phi=x(6);theta=x(7);psi=x(8);
  
  jac(Ax,Ay,Az,
  	  Bx,By,Bz,
  	  c_x,c_y,c_z,
  	  cyphy_ekf_pole::fx,cyphy_ekf_pole::fy,cyphy_ekf_pole::px,cyphy_ekf_pole::py,  
  	  phi,theta,psi, 
  	  jac_pt0);

  

  jac(Cx,Cy,Cz,
  	  Dx,Dy,Dz,
  	  c_x,c_y,c_z,
  	  cyphy_ekf_pole::fx,cyphy_ekf_pole::fy,cyphy_ekf_pole::px,cyphy_ekf_pole::py,  
  	  phi,theta,psi, 
  	  jac_pt1);

  for(int i=0;i<2;i++) for(int j=0;j<9;j++) ret(i,j) = jac_out0[i][j];
  for(int i=0;i<2;i++) for(int j=0;j<9;j++) ret(i+2,j) = jac_out1[i][j];
 
  ret(3,4) =0; 

  //cout<<"ret = "<<ret<<endl;
  return ret;	 
}

Matrix<float,4,1> cyphy_ekf_pole::LineModel_h(Matrix<float,9,1>x)
{
  Matrix<float,4,1> ret;
  ret.setZero();
  double line_out0[2][1];
  double line_out1[2][1];
  double (*line_pt0)[1];
  double (*line_pt1)[1];
  
  line_pt0=line_out0;
  line_pt1=line_out1;
  
  double Ax,Ay,Az,Bx,By,Bz,Cx,Cy,Cz,Dx,Dy,Dz,c_x,c_y,c_z,c_dx,c_dy,c_dz,phi,theta,psi;

  
  Ax=0;Ay=0;Az=0;
  Bx=0;By=0;Bz=-0.3;
  Cx=0;Cy=0.16;Cz=0;
  Dx=0;Dy=0.16;Dz=-0.3;
  
  c_x=x(0);c_y=x(1);c_z=x(2);
  c_dx=x(3);c_dy=x(4);c_dz=x(5);
  phi=x(6);theta=x(7);psi=x(8);
  
  line_model(Ax,Ay,Az,
	  	     Bx,By,Bz,
	  	     c_x,c_y,c_z,
		  	 cyphy_ekf_pole::fx,cyphy_ekf_pole::fy,cyphy_ekf_pole::px,cyphy_ekf_pole::py,   
		  	 phi,theta,psi,  
		  	 line_pt0);

  line_model(Cx,Cy,Cz,
	  	     Dx,Dy,Dz,
	  	     c_x,c_y,c_z,
		  	 cyphy_ekf_pole::fx,cyphy_ekf_pole::fy,cyphy_ekf_pole::px,cyphy_ekf_pole::py,    
		  	 phi,theta,psi,  
		  	 line_pt1);
  


  for(int i=0;i<2;i++) for(int j=0;j<1;j++) ret(i,j) = line_pt0[i][j];
  for(int i=0;i<2;i++) for(int j=0;j<1;j++) ret(i+2,j) = line_pt1[i][j];

  //cout<<"lin ret = "<<ret<<endl;
  return ret;
  

}

